#include <cmath>
#include <cfloat>
#include <cassert>

//#include "System.h"
#include "Mathutil.h"
#include "Quaternion.h"
#include "AABB.h"
#include "Matrix33.h"

using namespace gti;

void gti::Lerp(const vector3f &a, const vector3f &b, float percent, vector3f &c)
{
	c = a + (b-a)*percent;
}

void gti::Hermite(const vector3f &p0, const vector3f &p1,const vector3f &p2,const vector3f &p3, float t, vector3f& res)
{
	const float t2 = t*t;
	const float t3 = t2*t;

	const float alpha = 0.0f;//maximum tension
	const float aa = ((1.0f - alpha)*0.5f);
	
	const vector3f& m0 = (aa * ((p1-p0) + (p2-p1)));
	const vector3f& m1 = (aa * ((p2-p1) + (p3-p2)));

	res = ( (2*t3) -(3*t2) + 1.0f)*p1 + (t3 -(2*t2) + t)*m0 + (t3 - t2)*m1 + ((-2*t3) + (3*t2))*p2;
}

float gti::AngleBetweenVectors(const vector3f &v, const vector3f &w)
{
	// We assume we have normalized vectors
	// Here is the equation:  angle =  arc cosine of (V . W)
	// The Dot Product of returns the cosine of the angle between 2 vectors

	float dotProduct = DotProduct(v,w);

	// If our vectors weren't normalized, we must divide the dot product
	// by the magnitude of the 2 vectors multiplied by each other
	// angle =  arc cosine of (V . W / || V || * || W || )
	dotProduct /= (v.length() * w.length());

	float angle = acosf(dotProduct);

	// Here we make sure that the angle is not a -1.#IND0000000 number, 
	// which means indefinite.
	// Usually this value is found when the dot product and the maginitude 
	// are the same value.
	// We want to return 0 when this happens.
	if(std::isnan(angle)) return 0;
	
	// Return the angle in radians
	return angle;
}

bool gti::SameSide(const vector3f& p1, const vector3f& p2, const vector3f& a,const vector3f& b)
{
	const vector3f& ba = b-a;
    const vector3f& cp1 = CrossProduct(ba, p1-a);
    const vector3f& cp2 = CrossProduct(ba, p2-a);
    return (DotProduct(cp1, cp2) >= 0.0f);
}

bool gti::PointInTriangle(const vector3f& p, 
						 const vector3f& a, const vector3f& b, const vector3f& c)
{
    return ( SameSide(p,a, b,c) && SameSide(p,b, a,c)
        && SameSide(p,c, a,b) );
}

bool gti::PointInQuad(const vector3f& p, 
				 const vector3f& a, const vector3f& b, const vector3f& c, const vector3f& d)
{
	if (PointInTriangle(p,a,b,c) ||
		PointInTriangle(p,c,d,a))
		return true;

	return false;
	/*
    return (
		SameSide(p,c,a,b) && SameSide(p,d,b,c) && 
		SameSide(p,a,b,c) && SameSide(p,b,c,d)
		);
		*/
		
}

bool gti::PointInTriangleList(
						const vector3f& point,
						const vector3f* vertices,
						unsigned int verticesCnt)
{
	const unsigned int numberOfTriangles = verticesCnt/3;
	for (unsigned int i=0; i < numberOfTriangles; i+=3)
	{
		if (PointInTriangle(point,vertices[i],vertices[i+1],vertices[i+2]))
			return false;
	}
	return true;

/*
	double angle = 0.0;
	vector3f vA, vB;
	
	// It's a brilliant method really, what it does is create triangles
	// within the polygon from the intersection point.
	// It then adds up the inner angle of each of those triangles.
	// If the angles together add up to 360 degrees (or 2 * PI in radians) then 
	// we are inside!
	// If the angle is under that value, we must be outside of polygon.

	// Go in a circle to each vertex and get the angle between
	for (unsigned int i=0; i < verticesCnt; i++)
	{	
		// Subtract the intersection point from the current vertex
		vA = polygon[i] - point;
	  // Subtract the point from the next vertex
		vB = polygon[(i+1) % verticesCnt] - point;
												
		angle += AngleBetweenVectors(vA, vB);
	}

	// If the angle is greater than 2 PI, (360 degrees)
	return (angle >= (2.0*M_PI) - 1e-5 );
*/
}


void gti::ClosestPointOnLine(//Line p0-p1
		const vector3f& p0,// point p0
		const vector3f& p1,// point p1
		const vector3f& p,//Point p
		vector3f& r)// r is closest point on line
{
   // Determine t (the length of the vector from ëaí to ëpí)

//	projection of vector a over vector b
//	proA_B = a . b
//           ------ * b
//            b.b

   const vector3f& a = p - p0;
   vector3f b = p1 - p0;
   const float t = DotProduct (a,b);
   const float d = DotProduct(b,b);
   r = p0 + (t/d)*b;
}

void gti::ClosestPointOnSegment(//Line p0-p1
		const vector3f& p0,// point p0
		const vector3f& p1,// point p1
		const vector3f& p,//Point p
		vector3f& r)// r is closest point on line
{
   // Determine t (the length of the vector from ëaí to ëpí)

//	projection of vector a over vector b
//	proA_B = a . b
//           ------ * b
//            b.b

   const vector3f& a = p  - p0;
   const vector3f& b = p1 - p0;
   const float t = DotProduct (a,b);
   const float d = DotProduct(b,b);

   if (t<0.0f)
   {
	   r = p0;
	   return;
   }

   if (t>d)
   {
	   r = p1;
	   return;
   }

   r = p0 + (t/d)*b;
}

void gti::ClosestPointOnTriangle(//Triangle a,b,c
		const vector3f& a,//triangle first vertex
		const vector3f& b,//triangle second vertex
		const vector3f& c,//triangle third vertex
		const vector3f& p,//point p
		vector3f& r)// r is closest point on triangle
{
	vector3f points[3];
	// determine nearest point of every triangle edge and p
	ClosestPointOnSegment(a, b, p, points[0]);
	ClosestPointOnSegment(b, c, p, points[1]);
	ClosestPointOnSegment(c, a, p, points[2]);

	// determine nearest point respect point p
	float len[3];
	len[0] = (points[0] - p).squaredLength();
	len[1] = (points[1] - p).squaredLength();
	len[2] = (points[2] - p).squaredLength();

	int j;
	if (len[0] < len[1]) j=0;else j=1;
	if (len[2] < len[j]) j=2;

	// assign point
	r = points[j];
}

// http://www.gamasutra.com/features/19991018/Gomez_2.htm
// Return true if r1 and r2 are real
bool gti::QuadraticFormula
(
	const float a,
	const float b,
	const float c,
	float& r1, //first
	float& r2 //and second roots
)
{
	const float q = b*b - 4*a*c; 
	if( q >= 0.0f )
	{
		const float sq = sqrt(q);
		const float d = 1 / (2*a);
		r1 = ( -b + sq ) * d;
		r2 = ( -b - sq ) * d;
		return true;//real roots
	}
	else
	{
		return false;//complex roots
	}
}

float gti::EuclideanDistance(const vector3f& a, const vector3f& b)
{
	const float temp = (a.x-b.x)+(a.y-b.y)+(a.z-b.z);
	return temp*temp;
}

float gti::ManhattanDistance(const vector3f& a, const vector3f& b)
{
	return fabs(a.x-b.x) + fabs(a.y-b.y) + fabs(a.z-b.z);
}

void gti::ClosestPointsBetweenSegments(
		const vector3f& P0,
		const vector3f& D0,
		const vector3f& P1,
		const vector3f& D1,
		vector3f& Q0,
		vector3f& Q1)
{
	vector3f u = D0 - P0;
    vector3f v = D1 - P1;
    vector3f w = P0 - P1;
    float    a = DotProduct(u,u);        // always >= 0
    float    b = DotProduct(u,v);
    float    c = DotProduct(v,v);        // always >= 0
    float    d = DotProduct(u,w);
    float    e = DotProduct(v,w);
    float    D = a*c - b*b;       // always >= 0
    float    sc, sN, sD = D;      // sc = sN / sD, default sD = D >= 0
    float    tc, tN, tD = D;      // tc = tN / tD, default tD = D >= 0

    // compute the line parameters of the two closest points
    if (D < 1e-05) { // the lines are almost parallel
        sN = 0.0;        // force using point P0 on segment S1
        sD = 1.0;        // to prevent possible division by 0.0 later
        tN = e;
        tD = c;
    }
    else {                // get the closest points on the infinite lines
        sN = (b*e - c*d);
        tN = (a*e - b*d);
        if (sN < 0.0) {       // sc < 0 => the s=0 edge is visible
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if (sN > sD) {  // sc > 1 => the s=1 edge is visible
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0) {           // tc < 0 => the t=0 edge is visible
        tN = 0.0;
        // recompute sc for this edge
        if (-d < 0.0)
            sN = 0.0;
        else if (-d > a)
            sN = sD;
        else {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD) {      // tc > 1 => the t=1 edge is visible
        tN = tD;
        // recompute sc for this edge
        if ((-d + b) < 0.0)
            sN = 0;
        else if ((-d + b) > a)
            sN = sD;
        else {
            sN = (-d + b);
            sD = a;
        }
    }
    // finally do the division to get sc and tc
    sc = (fabsf(sN) < 1e-05f ? 0.0f : sN / sD);
    tc = (fabsf(tN) < 1e-05f ? 0.0f : tN / tD);

    // get the difference of the two closest points
    vector3f   dP = w + (sc * u) - (tc * v);  // = S1(sc) - S2(tc)
//    float len = dP.normalize();   // return the closest distance

	Q0 = P0 + sc*u;
	Q1 = P1 + tc*v;
}

bool gti::SegmentOverlapAABB(const vector3f& mid, const vector3f& l, const float hl, const AABB& aabb)
{
	float r;
	const vector3f& c = aabb.center;
	const vector3f& hs = aabb.halfsize;

/* ALGORITHM: Use the separating axis 
theorem to see if the line segment 
and the box overlap. A line 
segment is a degenerate OBB. */

	const vector3f& T = c - mid;

//do any of the principal axes
//form a separating axis?

	if( fabs(T.x) > hs.x + hl*fabs(l.x) )
		return false;

	if( fabs(T.y) > hs.y + hl*fabs(l.y) )
		return false;

	if( fabs(T.z) > hs.z + hl*fabs(l.z) )
		return false;

/* NOTE: Since the separating axis is
perpendicular to the line in these
last four cases, the line does not
contribute to the projection. */

//l.cross(x-axis)?

	r = hs.y*fabs(l.z) + hs.z*fabs(l.y);

	if( fabs(T.y*l.z - T.z*l.y) > r )
		return false;

	//l.cross(y-axis)?

	r = hs.x*fabs(l.z) + hs.z*fabs(l.x);

	if( fabs(T.z*l.x - T.x*l.z) > r )
		return false;

	//l.cross(z-axis)?

	r = hs.x*fabs(l.y) + hs.y*fabs(l.x);

	if( fabs(T.x*l.y - T.y*l.x) > r )
		return false;

	return true;
}


bool gti::SegmentOverlapAABB(const vector3f& a, const vector3f& b, const AABB& aabb)
{
	vector3f l = b-a;
	float hl = l.normalize() * 0.5f;
	const vector3f& mid = (a+b)*0.5f;

	return SegmentOverlapAABB(mid,l,hl,aabb);
}
////////////////////////////////////////////////////////////////////////////////
#define X 0
#define Y 1
#define Z 2

#define FINDMINMAX(x0,x1,min,max) \
  min = max = x0;   \
  if(x1<min) min=x1;\
  if(x1>max) max=x1;

/*======================== X-tests ========================*/

#define AXISTEST_X2(a, b, fa, fb)			   \
	p0 = a*v0[Y] - b*v0[Z];			           \
	p1 = a*v1[Y] - b*v1[Z];			       	   \
        if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * halfsize[Y] + fb * halfsize[Z];   \
	if(min>rad || max<-rad) return false;

/*======================== Y-tests ========================*/

#define AXISTEST_Y1(a, b, fa, fb)			   \
	p0 = -a*v0[X] + b*v0[Z];		      	   \
	p1 = -a*v1[X] + b*v1[Z];	     	       	   \
        if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * halfsize[X] + fb * halfsize[Z];   \
	if(min>rad || max<-rad) return false;

/*======================== Z-tests ========================*/

#define AXISTEST_Z0(a, b, fa, fb)			   \
	p0 = a*v0[X] - b*v0[Y];				   \
	p1 = a*v1[X] - b*v1[Y];			           \
        if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * halfsize[X] + fb * halfsize[Y];   \
	if(min>rad || max<-rad) return false;

bool gti::FastSegmentOverlapAABB(const vector3f& a, const vector3f& b,
							const vector3f& center, const vector3f& halfsize )
{
	const vector3f& v0 = a-center;
	const vector3f& v1 = b-center;

	return FastSegmentOverlapAABB(v0,v1,halfsize );
}

bool gti::FastSegmentOverlapAABB(const vector3f& v0, const vector3f& v1,const vector3f& halfsize )
{
	// portions of Moller triangle-aabb test copied and adapted (EGB)

	// use the separating axis theorem to see whether 
	// the segment and the aabb overlap
	
	float min,max,p0,p1,rad,fex,fey,fez;	
	const vector3f& e0 = v1-v0;      // tri edge 0 

	// Bullet 3:
	//  test the 3 tests first (this was faster)
	fex = fabsf(e0[X]);
	fey = fabsf(e0[Y]);
	fez = fabsf(e0[Z]);

	AXISTEST_X2(e0[Z], e0[Y], fez, fey);
	AXISTEST_Y1(e0[Z], e0[X], fez, fex);
	AXISTEST_Z0(e0[Y], e0[X], fey, fex);

	// test in X-direction
	FINDMINMAX(v0[X],v1[X],min,max);
	if(min>halfsize[X] || max<-halfsize[X]) return false;
	// test in Y-direction
	FINDMINMAX(v0[Y],v1[Y],min,max);
	if(min>halfsize[Y] || max<-halfsize[Y]) return false;
	// test in Z-direction
	FINDMINMAX(v0[Z],v1[Z],min,max);
	if(min>halfsize[Z] || max<-halfsize[Z]) return false;		

	return true;

}

////////////////////////////////////////////////////////////////////////////////

// given the halfsize and orientation of an OBB, 
// rescale the halfsize to world coordinate frame
// (NOTE: the center is the same for both)
void gti::OBBtoAABB(const vector3f& ihs, const quaternion& q, vector3f& ohs)
{
	matrix33 R;
	q.toMatrix(R);
	OBBtoAABB(ihs, R, ohs);
}

void gti::OBBtoAABB(const vector3f& ihs, const matrix33& R, vector3f& ohs)
{
	ohs.set(fabs(R._11)*ihs.x+fabs(R._21)*ihs.y+fabs(R._31)*ihs.z,
				fabs(R._12)*ihs.x+fabs(R._22)*ihs.y+fabs(R._32)*ihs.z,
				fabs(R._13)*ihs.x+fabs(R._23)*ihs.y+fabs(R._33)*ihs.z
				);
}

// http://www.gamasutra.com/features/19991018/Gomez_5.htm
bool gti::OBBIntersectionTest(const vector3f& CA, const matrix33& RA, const vector3f& HA,
												 const vector3f& CB, const matrix33& RB, const vector3f& HB)
{
	//translation, in parent frame
	const vector3f& v = CB - CA;
	
	//translation, in A's frame
	vector3f T( 
		DotProduct(v, RA.axis(0)), 
		DotProduct(v, RA.axis(1)),
		DotProduct(v, RA.axis(2))
	);

	//B's basis with respect to A's local frame
	float R[3][3];
	float ra, rb, t;
	int i, k;

	//calculate rotation matrix
	for( i=0 ; i<3 ; i++ )
	for( k=0 ; k<3 ; k++ )
		R[i][k] = DotProduct( RA.axis(i), RB.axis(k) ); 
	
	/*ALGORITHM: Use the separating axis test for all 15 potential 
	separating axes. If a separating axis could not be found, the two 
	boxes overlap. */

	// 6 tests are the basis vectors of A and B
	// 9 are the cross product of all the edges

	//A's basis vectors
	for( i=0 ; i<3 ; i++ )
	{
		ra = HA[i];

		rb = 
		HB[0]*fabsf(R[i][0]) + HB[1]*fabsf(R[i][1]) + HB[2]*fabsf(R[i][2]);

		t = fabsf( T[i] );

		if( t > ra + rb ) 
			return false;
	}

	//B's basis vectors
	for( k=0 ; k<3 ; k++ )
	{
		ra = HA[0]*fabsf(R[0][k]) + HA[1]*fabsf(R[1][k]) + HA[2]*fabsf(R[2][k]);

		rb = HB[k];

		t =  fabsf( T[0]*R[0][k] + T[1]*R[1][k] +  T[2]*R[2][k] );

		if( t > ra + rb )
			return false;

	}

	//9 cross products

	//L = A0 x B0
	ra = HA[1]*fabsf(R[2][0]) + HA[2]*fabsf(R[1][0]);

	rb = HB[1]*fabsf(R[0][2]) + HB[2]*fabsf(R[0][1]);

	t = fabsf( T[2]*R[1][0] -  T[1]*R[2][0] );

	if( t > ra + rb )
		return false;

	//L = A0 x B1
	ra = HA[1]*fabsf(R[2][1]) + HA[2]*fabsf(R[1][1]);

	rb = HB[0]*fabsf(R[0][2]) + HB[2]*fabsf(R[0][0]);

	t = fabsf( T[2]*R[1][1] -  T[1]*R[2][1] );

	if( t > ra + rb )
		return false;

	//L = A0 x B2
	ra = HA[1]*fabsf(R[2][2]) + HA[2]*fabsf(R[1][2]);

	rb = HB[0]*fabsf(R[0][1]) + HB[1]*fabsf(R[0][0]);

	t = fabsf( T[2]*R[1][2] - T[1]*R[2][2] );

	if( t > ra + rb )
		return false;

	//L = A1 x B0
	ra = HA[0]*fabsf(R[2][0]) + HA[2]*fabsf(R[0][0]);

	rb = HB[1]*fabsf(R[1][2]) + HB[2]*fabsf(R[1][1]);

	t = fabsf( T[0]*R[2][0] -  T[2]*R[0][0] );

	if( t > ra + rb )
		return false;

	//L = A1 x B1
	ra = HA[0]*fabsf(R[2][1]) + HA[2]*fabsf(R[0][1]);

	rb = HB[0]*fabsf(R[1][2]) + HB[2]*fabsf(R[1][0]);

	t = fabsf( T[0]*R[2][1] - T[2]*R[0][1] );

	if( t > ra + rb )
		return false;

	//L = A1 x B2
	ra = HA[0]*fabsf(R[2][2]) + HA[2]*fabsf(R[0][2]);

	rb = HB[0]*fabsf(R[1][1]) + HB[1]*fabsf(R[1][0]);

	t = fabsf( T[0]*R[2][2] - T[2]*R[0][2] );

	if( t > ra + rb )
		return false;

	//L = A2 x B0
	ra = HA[0]*fabsf(R[1][0]) + HA[1]*fabsf(R[0][0]);

	rb = HB[1]*fabsf(R[2][2]) + HB[2]*fabsf(R[2][1]);

	t = fabsf( T[1]*R[0][0] - T[0]*R[1][0] );

	if( t > ra + rb )
	return false;

	//L = A2 x B1
	ra = HA[0]*fabsf(R[1][1]) + HA[1]*fabsf(R[0][1]);

	rb = HB[0] *fabsf(R[2][2]) + HB[2]*fabsf(R[2][0]);

	t = fabsf( T[1]*R[0][1] - T[0]*R[1][1] );

	if( t > ra + rb )
	return false;

	//L = A2 x B2
	ra = HA[0]*fabsf(R[1][2]) + HA[1]*fabsf(R[0][2]);

	rb = HB[0]*fabsf(R[2][1]) + HB[1]*fabsf(R[2][0]);

	t = fabsf( T[1]*R[0][2] - T[0]*R[1][2] );

	if( t > ra + rb )
	return false;

	/*no separating axis found,
	the two boxes overlap */

	return true;
}

// AABB-AABB Intersection Test
bool AABBIntersectionTest(const vector3f& CA, const vector3f& HA, 
													const vector3f& CB, const vector3f& HB)
{
	const float x =  fabsf(CB.x - CA.x);
	if (x > (HA.x + HB.x)) return false;
	const float y =  fabsf(CB.y - CA.y);
	if (y > (HA.y + HB.y)) return false;
	const float z =  fabsf(CB.z - CA.z);
	if (z > (HA.z + HB.z)) return false;

	return true;
}


bool gti::PointAABBTest(const vector3f& point, const vector3f& center, const vector3f& halfsize)
{
	return  std::abs(point.x - center.x) <= halfsize.x &&
				  std::abs(point.y - center.y) <= halfsize.y &&
				  std::abs(point.z - center.z) <= halfsize.z;
}

#undef X
#undef Y
#undef Z
#undef FINDMINMAX
#undef AXISTEST_X2
#undef AXISTEST_Y1
#undef AXISTEST_Z0
////////////////////////////////////////////////////////////////////////////////

void gti::computeQuadtreeNodeVolume(char nodeID, const AABB& volume, AABB& childVolume)
{
	childVolume.halfsize.x = volume.halfsize.x/2.0f;
	childVolume.halfsize.y = volume.halfsize.y;
	childVolume.halfsize.z = volume.halfsize.z/2.0f;

	const vector3f& c = volume.center;
	const vector3f& halfsize = childVolume.halfsize;
	
	// Switch on the ID to see which subdivided node we are finding the center
	switch(nodeID)							
	{
		case TOP_LEFT:
			// Calculate the center of this new node
			childVolume.center.set(c.x - halfsize.x, c.y, c.z + halfsize.z);
			break;

		case TOP_RIGHT:
			// Calculate the center of this new node
			childVolume.center.set(c.x + halfsize.x, c.y, c.z + halfsize.z);
			break;

		case BOTTOM_LEFT:
			// Calculate the center of this new node
			childVolume.center.set(c.x - halfsize.x, c.y, c.z - halfsize.z);
			break;

		case BOTTOM_RIGHT:
			// Calculate the center of this new node
			childVolume.center.set(c.x + halfsize.x, c.y, c.z - halfsize.z);
			break;
	}
}

void gti::computeOctreeNodeVolume(char nodeID, const AABB& volume, AABB& childVolume)
{
	childVolume.halfsize = volume.halfsize / 2.0f;

	const vector3f & c = volume.center;
	const vector3f & halfsize = childVolume.halfsize;
	
	// Switch on the ID to see which subdivided node we are finding the center
	switch(nodeID)							
	{
		case TOP_LEFT_FRONT:
			// Calculate the center of this new node
			childVolume.center.set(c.x - halfsize.x, c.y + halfsize.y, c.z + halfsize.z);
			break;

		case TOP_LEFT_BACK:
			// Calculate the center of this new node
			childVolume.center.set(c.x - halfsize.x, c.y + halfsize.y, c.z - halfsize.z);
			break;

		case TOP_RIGHT_BACK:
			// Calculate the center of this new node
			childVolume.center.set(c.x + halfsize.x, c.y + halfsize.y, c.z - halfsize.z);
			break;

		case TOP_RIGHT_FRONT:
			// Calculate the center of this new node
			childVolume.center.set(c.x + halfsize.x, c.y + halfsize.y, c.z + halfsize.z);
			break;

		case BOTTOM_LEFT_FRONT:
			// Calculate the center of this new node
			childVolume.center.set(c.x - halfsize.x, c.y - halfsize.y, c.z + halfsize.z);
			break;

		case BOTTOM_LEFT_BACK:
			// Calculate the center of this new node
			childVolume.center.set(c.x - halfsize.x, c.y - halfsize.y, c.z - halfsize.z);
			break;

		case BOTTOM_RIGHT_BACK:
			// Calculate the center of this new node
			childVolume.center.set(c.x + halfsize.x, c.y - halfsize.y, c.z - halfsize.z);
			break;

		case BOTTOM_RIGHT_FRONT:
			// Calculate the center of this new node
			childVolume.center.set(c.x + halfsize.x, c.y - halfsize.y, c.z + halfsize.z);
			break;
	}
}

// --------------------------------------------------------
float gti::rad2deg( float rad )
{
	return ( 180.0f*rad/(float)M_PI );
}

float gti::deg2rad( float deg )
{
	return ( deg*(float)M_PI/180.0f );
}

vector3f gti::angles2axis( float yaw, float pitch ) {
	return vector3f ( sinf( yaw ) * cosf( pitch )
							    ,             - sinf( pitch )
							    , cosf( yaw ) * cosf( pitch )
							    );
}

void gti::axis2angles( const vector3f &axis, float *yaw, float *pitch ) {
	*yaw = atan2f( axis.x, axis.z );
	const float mdo = sqrtf( axis.x * axis.x + axis.z * axis.z );
	*pitch = atan2f( -axis.y, mdo );
}

float gti::getAnglePlaneXZ( const vector3f &axis ) {
	return atan2f( axis.x, axis.z );
}

vector3f gti::getVectorXZOfAngle( float angle ) {
	return vector3f( sin(angle), 0, cos(angle) );
}

vector3f gti::leftOf( const vector3f &p ) {
	return vector3f( p.z, 0.0f, -p.x );
}

vector3f gti::rightOf( const vector3f &p ) {
	return vector3f( -p.z, 0.0f, p.x );
}

quaternion gti::getRotationBetweenDirections( const vector3f &prev_dir, const vector3f &new_dir, float ratio ) {
  assert( ratio >= 0.0f && ratio <= 1.0f );
  vector3f axis_rotation = CrossProduct( prev_dir, new_dir );
  if( axis_rotation.length( ) > 1e-3 ) {
    
	// Normalize the given vectors
	vector3f nprev_dir( prev_dir );
	nprev_dir.normalize( );
    vector3f nnew_dir( new_dir );
    nnew_dir.normalize( );
	
    axis_rotation.normalize( );
    float cos_angle_rotation = DotProduct( nprev_dir, nnew_dir );
    float angle_rotation = acos( cos_angle_rotation );
    angle_rotation *= ratio;
    return quaternion( axis_rotation, -angle_rotation );
  }
  return quaternion( 0, 0, 0, 1 );
}

void gti::computeVolume(unsigned int count, const gti::vector3f* vertices, gti::AABB& volume)
{
	unsigned int i,j;
	gti::vector3f vmin(FLT_MAX,FLT_MAX,FLT_MAX);
	gti::vector3f vmax(-FLT_MAX,-FLT_MAX,-FLT_MAX);

	for (i=0;i<count;i++)
	{
		const gti::vector3f &v = vertices[i];
		for (j=0;j<3;j++)
		{
			if (vmax[j] < v[j]) vmax[j]=v[j];
			if (vmin[j] > v[j])  vmin[j]=v[j];
		}
	}
	volume.setMinMax(vmin,vmax);
}

void gti::computeVolume(const std::vector<gti::vector3f>& vertices, gti::AABB& volume)
{
	assert(vertices.size()>0);
	if (vertices.size()>0)
	{
		gti::computeVolume(vertices.size(),&vertices[0],volume);
	}
}
